#include "uni_servo.h"


/****************************
 构造函数，声明1个舵机对象
 _pin1: 声明的第1个舵机引脚号
****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1){
  _uni_servo_label[0] = _pin1;
  _uni_servo_current_count = 1;
}

/****************************
 构造函数，声明2个舵机对象
 _pin1: 声明的第1个舵机引脚号
 _pin2: 声明的第2个舵机引脚号
****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2){
  uint8_t servo_pins[] = {_pin1, _pin2};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/****************************
 构造函数，声明3个舵机对象
 _pin1: 声明的第1个舵机引脚号
 _pin2: 声明的第2个舵机引脚号
 _pin3: 声明的第3个舵机引脚号
****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3){
  uint8_t servo_pins[] = {_pin1, _pin2, _pin3};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/****************************
 构造函数，声明4个舵机对象
 _pin1: 声明的第1个舵机引脚号
 _pin2: 声明的第2个舵机引脚号
 _pin3: 声明的第3个舵机引脚号
 _pin4: 声明的第4个舵机引脚号
****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4){
  uint8_t servo_pins[] = {_pin1, _pin2, _pin3, _pin4};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/****************************
 构造函数，声明5个舵机对象
 _pin1: 声明的第1个舵机引脚号
 _pin2: 声明的第2个舵机引脚号
 _pin3: 声明的第3个舵机引脚号
 _pin4: 声明的第4个舵机引脚号
 _pin5: 声明的第5个舵机引脚号
****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5){
  uint8_t servo_pins[] = {_pin1, _pin2, _pin3, _pin4, _pin5};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/****************************
 构造函数，声明6个舵机对象
 _pin1: 声明的第1个舵机引脚号
 _pin2: 声明的第2个舵机引脚号
 _pin3: 声明的第3个舵机引脚号
 _pin4: 声明的第4个舵机引脚号
 _pin5: 声明的第5个舵机引脚号
 _pin6: 声明的第6个舵机引脚号
****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6){
  uint8_t servo_pins[] = {_pin1, _pin2, _pin3, _pin4, _pin5, _pin6};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/****************************
 构造函数，声明7个舵机对象
 _pin1: 声明的第1个舵机引脚号
 _pin2: 声明的第2个舵机引脚号
 _pin3: 声明的第3个舵机引脚号
 _pin4: 声明的第4个舵机引脚号
 _pin5: 声明的第5个舵机引脚号
 _pin6: 声明的第6个舵机引脚号
 _pin7: 声明的第7个舵机引脚号
****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7){
  uint8_t servo_pins[] = {_pin1, _pin2, _pin3, _pin4, _pin5, _pin6, _pin7};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/****************************
 构造函数，声明8个舵机对象
 _pin1: 声明的第1个舵机引脚号
 _pin2: 声明的第2个舵机引脚号
 _pin3: 声明的第3个舵机引脚号
 _pin4: 声明的第4个舵机引脚号
 _pin5: 声明的第5个舵机引脚号
 _pin6: 声明的第6个舵机引脚号
 _pin7: 声明的第7个舵机引脚号
 _pin8: 声明的第8个舵机引脚号
****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7, uint8_t _pin8){
  uint8_t servo_pins[] = {_pin1, _pin2, _pin3, _pin4, _pin5, _pin6, _pin7, _pin8};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/****************************
 构造函数，声明9个舵机对象
 _pin1: 声明的第1个舵机引脚号
 _pin2: 声明的第2个舵机引脚号
 _pin3: 声明的第3个舵机引脚号
 _pin4: 声明的第4个舵机引脚号
 _pin5: 声明的第5个舵机引脚号
 _pin6: 声明的第6个舵机引脚号
 _pin7: 声明的第7个舵机引脚号
 _pin8: 声明的第8个舵机引脚号
 _pin9: 声明的第9个舵机引脚号
****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7, uint8_t _pin8, uint8_t _pin9){
  uint8_t servo_pins[] = {_pin1, _pin2, _pin3, _pin4, _pin5, _pin6, _pin7, _pin8, _pin9};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/*****************************
 构造函数，声明10个舵机对象
 _pin1:  声明的第1个舵机引脚号
 _pin2:  声明的第2个舵机引脚号
 _pin3:  声明的第3个舵机引脚号
 _pin4:  声明的第4个舵机引脚号
 _pin5:  声明的第5个舵机引脚号
 _pin6:  声明的第6个舵机引脚号
 _pin7:  声明的第7个舵机引脚号
 _pin8:  声明的第8个舵机引脚号
 _pin9:  声明的第9个舵机引脚号
 _pin10: 声明的第10个舵机引脚号
*****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7, uint8_t _pin8, uint8_t _pin9, uint8_t _pin10){
  uint8_t servo_pins[] = {_pin1, _pin2, _pin3, _pin4, _pin5, _pin6, _pin7, _pin8, _pin9, _pin10};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/*****************************
 构造函数，声明11个舵机对象
 _pin1:  声明的第1个舵机引脚号
 _pin2:  声明的第2个舵机引脚号
 _pin3:  声明的第3个舵机引脚号
 _pin4:  声明的第4个舵机引脚号
 _pin5:  声明的第5个舵机引脚号
 _pin6:  声明的第6个舵机引脚号
 _pin7:  声明的第7个舵机引脚号
 _pin8:  声明的第8个舵机引脚号
 _pin9:  声明的第9个舵机引脚号
 _pin10: 声明的第10个舵机引脚号
 _pin11: 声明的第11个舵机引脚号
*****************************/
UNI_SERVO::UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7, uint8_t _pin8, uint8_t _pin9, uint8_t _pin10, uint8_t _pin11){
  uint8_t servo_pins[] = {_pin1, _pin2, _pin3, _pin4, _pin5, _pin6, _pin7, _pin8, _pin9, _pin10, _pin11};
  uint8_t num_servos = sizeof(servo_pins) / sizeof(servo_pins[0]);
  _uni_servo_current_count = num_servos;
  for(uint8_t i=0;i<num_servos;i++){
    _uni_servo_label[i] = servo_pins[i];
  }
}

/*********************************************************
 析构函数，目前该函数功能暂未开放，后续开发可能会添加该函数功能
**********************************************************/
UNI_SERVO::~UNI_SERVO(){

}

/*********
 使能舵机
*********/
void UNI_SERVO::uni_servo_attach_start(){
  for(int i=0; i<_uni_servo_current_count; i++){
    _servos[i].attach(_uni_servo_label[i]);
  }
}

/************
 舵机初始化
************/
void UNI_SERVO::init(){
  uni_servo_attach_start();
}

/****************************************************************
功能：指定180舵机编号的舵机转动函数
函数原型：void pin_move180(int label1, int angle);
参数释义：
    int label: 舵机编号.要查看舵机编号,请阅读该库文件的引脚图说明部分.
               本程序包含的舵机引脚为：2,3,4,6,44,45,46,7,10,11,13
    int angle: 舵机需要转动到的角度.
*****************************************************************/
void UNI_SERVO::pin_move180(int label1, int angle) {
  for (int i = 0; i < _uni_servo_current_count; i++) {
    if (_uni_servo_label[i] == label1) {
      if(angle <=0){angle = 0;}
      if(angle >=180){angle = 180;}
      _servos[i].write(angle);
      break;
    }
  }
}

/****************************************************************
功能：指定270舵机编号的舵机转动函数
函数原型：void pin_move270(int servo_pin, float angle);
参数释义：
    int label: 舵机编号.要查看舵机编号,请阅读该库文件的引脚图说明部分.
               本程序包含的舵机引脚为：2,3,4,6,44,45,46,7,10,11,13
    int angle: 舵机需要转动到的角度.
*****************************************************************/
void UNI_SERVO::pin_move270(int servo_pin, float angle){
  for (int i = 0; i < _uni_servo_current_count; i++) {
    if (_uni_servo_label[i] == servo_pin) {
      if(angle <=0){angle = 0;}
      if(angle >=270){angle = 270;}
      int pulse = map(angle, 0, 270, 500, 2500);
      _servos[i].writeMicroseconds(pulse);
      break;
    }
  }
}

/****************************************************************
功能：指定舵机编号的舵机转动函数
函数原型：void pin_movemap(int servo_pin, float angle);
参数释义：
    int label: 舵机编号.要查看舵机编号,请阅读该库文件的引脚图说明部分.
               本程序包含的舵机引脚为：2,3,4,6,44,45,46,7,10,11,13
    int angle: 舵机需要转动到的角度.
*****************************************************************/
void UNI_SERVO::pin_movemap(int servo_pin, float angle){
  for (int i = 0; i < _uni_servo_current_count; i++) {
    if (_uni_servo_label[i] == servo_pin) {
      if(angle <=500){angle = 500;}
      if(angle >=2500){angle = 2500;}
      _servos[i].writeMicroseconds(angle);
      break;
    }
  }
}

//=========================================================================================================================
//================== 180度舵机转动函数 =====================================================================================
//=========================================================================================================================
/*******************************************************************
控制舵机转动函数
  -若仅声明了1个舵机，调用该函数，可直接控制单个舵机需要转动到的角度
  -若声明了多个舵机，调用该函数时，此时仅声明的第一个舵机会转动
********************************************************************/
void UNI_SERVO::move180(int ang1){
  int _uni_temporary = ang1;
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary);
  }
}

/**********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
***********************************************/
void UNI_SERVO::move180(int ang1, int ang2){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/***********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
************************************************/
void UNI_SERVO::move180(int ang1, int ang2, int ang3){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move180(int ang1, int ang2, int ang3, int ang4){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move180(int ang1, int ang2, int ang3, int ang4, int ang5){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
  ang7:声明的第7个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
  ang7:声明的第7个舵机需要转动的角度
  ang8:声明的第8个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
  ang7:声明的第7个舵机需要转动的角度
  ang8:声明的第8个舵机需要转动的角度
  ang9:声明的第9个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8, ang9};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1: 声明的第1个舵机需要转动的角度
  ang2: 声明的第2个舵机需要转动的角度
  ang3: 声明的第3个舵机需要转动的角度
  ang4: 声明的第4个舵机需要转动的角度
  ang5: 声明的第5个舵机需要转动的角度
  ang6: 声明的第6个舵机需要转动的角度
  ang7: 声明的第7个舵机需要转动的角度
  ang8: 声明的第8个舵机需要转动的角度
  ang9: 声明的第9个舵机需要转动的角度
  ang10:声明的第10个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8, ang9, ang10};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1: 声明的第1个舵机需要转动的角度
  ang2: 声明的第2个舵机需要转动的角度
  ang3: 声明的第3个舵机需要转动的角度
  ang4: 声明的第4个舵机需要转动的角度
  ang5: 声明的第5个舵机需要转动的角度
  ang6: 声明的第6个舵机需要转动的角度
  ang7: 声明的第7个舵机需要转动的角度
  ang8: 声明的第8个舵机需要转动的角度
  ang9: 声明的第9个舵机需要转动的角度
  ang10:声明的第10个舵机需要转动的角度
  ang11:声明的第11个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10, int ang11){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8, ang9, ang10, ang11};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move180(_uni_servo_label[i], _uni_temporary[i]);
  }
}



//=========================================================================================================================
//================== 270度舵机转动函数 =====================================================================================
//=========================================================================================================================
/*******************************************************************
控制舵机转动函数
  -若仅声明了1个舵机，调用该函数，可直接控制单个舵机需要转动到的角度
  -若声明了多个舵机，调用该函数时，此时仅声明的第一个舵机会转动
********************************************************************/
void UNI_SERVO::move270(int ang1){
  int _uni_temporary = ang1;
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary);
  }
}

/**********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
***********************************************/
void UNI_SERVO::move270(int ang1, int ang2){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/***********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
************************************************/
void UNI_SERVO::move270(int ang1, int ang2, int ang3){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move270(int ang1, int ang2, int ang3, int ang4){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move270(int ang1, int ang2, int ang3, int ang4, int ang5){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
  ang7:声明的第7个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
  ang7:声明的第7个舵机需要转动的角度
  ang8:声明的第8个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
  ang7:声明的第7个舵机需要转动的角度
  ang8:声明的第8个舵机需要转动的角度
  ang9:声明的第9个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8, ang9};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1: 声明的第1个舵机需要转动的角度
  ang2: 声明的第2个舵机需要转动的角度
  ang3: 声明的第3个舵机需要转动的角度
  ang4: 声明的第4个舵机需要转动的角度
  ang5: 声明的第5个舵机需要转动的角度
  ang6: 声明的第6个舵机需要转动的角度
  ang7: 声明的第7个舵机需要转动的角度
  ang8: 声明的第8个舵机需要转动的角度
  ang9: 声明的第9个舵机需要转动的角度
  ang10:声明的第10个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8, ang9, ang10};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1: 声明的第1个舵机需要转动的角度
  ang2: 声明的第2个舵机需要转动的角度
  ang3: 声明的第3个舵机需要转动的角度
  ang4: 声明的第4个舵机需要转动的角度
  ang5: 声明的第5个舵机需要转动的角度
  ang6: 声明的第6个舵机需要转动的角度
  ang7: 声明的第7个舵机需要转动的角度
  ang8: 声明的第8个舵机需要转动的角度
  ang9: 声明的第9个舵机需要转动的角度
  ang10:声明的第10个舵机需要转动的角度
  ang11:声明的第11个舵机需要转动的角度
*********************************************/
void UNI_SERVO::move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10, int ang11){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8, ang9, ang10, ang11};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_move270(_uni_servo_label[i], _uni_temporary[i]);
  }
}

//=========================================================================================================================
//================== 舵机500~2500脉宽转动函数 ===============================================================================
//=========================================================================================================================
/*******************************************************************
控制舵机转动函数
  -若仅声明了1个舵机，调用该函数，可直接控制单个舵机需要转动到的角度
  -若声明了多个舵机，调用该函数时，此时仅声明的第一个舵机会转动
********************************************************************/
void UNI_SERVO::movemap(int ang1){
  int _uni_temporary = ang1;
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary);
  }
}

/**********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
***********************************************/
void UNI_SERVO::movemap(int ang1, int ang2){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/***********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
************************************************/
void UNI_SERVO::movemap(int ang1, int ang2, int ang3){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
*********************************************/
void UNI_SERVO::movemap(int ang1, int ang2, int ang3, int ang4){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
*********************************************/
void UNI_SERVO::movemap(int ang1, int ang2, int ang3, int ang4, int ang5){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
*********************************************/
void UNI_SERVO::movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
  ang7:声明的第7个舵机需要转动的角度
*********************************************/
void UNI_SERVO::movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
  ang7:声明的第7个舵机需要转动的角度
  ang8:声明的第8个舵机需要转动的角度
*********************************************/
void UNI_SERVO::movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1:声明的第1个舵机需要转动的角度
  ang2:声明的第2个舵机需要转动的角度
  ang3:声明的第3个舵机需要转动的角度
  ang4:声明的第4个舵机需要转动的角度
  ang5:声明的第5个舵机需要转动的角度
  ang6:声明的第6个舵机需要转动的角度
  ang7:声明的第7个舵机需要转动的角度
  ang8:声明的第8个舵机需要转动的角度
  ang9:声明的第9个舵机需要转动的角度
*********************************************/
void UNI_SERVO::movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8, ang9};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1: 声明的第1个舵机需要转动的角度
  ang2: 声明的第2个舵机需要转动的角度
  ang3: 声明的第3个舵机需要转动的角度
  ang4: 声明的第4个舵机需要转动的角度
  ang5: 声明的第5个舵机需要转动的角度
  ang6: 声明的第6个舵机需要转动的角度
  ang7: 声明的第7个舵机需要转动的角度
  ang8: 声明的第8个舵机需要转动的角度
  ang9: 声明的第9个舵机需要转动的角度
  ang10:声明的第10个舵机需要转动的角度
*********************************************/
void UNI_SERVO::movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8, ang9, ang10};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/********************************************
控制舵机转动函数
  ang1: 声明的第1个舵机需要转动的角度
  ang2: 声明的第2个舵机需要转动的角度
  ang3: 声明的第3个舵机需要转动的角度
  ang4: 声明的第4个舵机需要转动的角度
  ang5: 声明的第5个舵机需要转动的角度
  ang6: 声明的第6个舵机需要转动的角度
  ang7: 声明的第7个舵机需要转动的角度
  ang8: 声明的第8个舵机需要转动的角度
  ang9: 声明的第9个舵机需要转动的角度
  ang10:声明的第10个舵机需要转动的角度
  ang11:声明的第11个舵机需要转动的角度
*********************************************/
void UNI_SERVO::movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10, int ang11){
  int _uni_temporary[_uni_servo_current_count] = {ang1, ang2, ang3, ang4, ang5, ang6, ang7, ang8, ang9, ang10, ang11};
  for(int i=0; i<_uni_servo_current_count; i++){
    pin_movemap(_uni_servo_label[i], _uni_temporary[i]);
  }
}

/*****************************
 指定_pin引脚的舵机失能
 _pin: 需要失能的舵机引脚号
******************************/
void UNI_SERVO::pin_detach(uint8_t _pin){
  for (int i = 0; i < _uni_servo_current_count; i++) {
    if (_uni_servo_label[i] == _pin) {
      _servos[i].detach();
      break;
    }
  }
}

/*****************************
所有舵机都失能
******************************/
void UNI_SERVO::detach(){
  for(int i=0; i<_uni_servo_current_count; i++){
    _servos[i].detach();
  }
}

/*****************************
在串口中打印当前定义的舵机参数
方便开发者校验舵机
******************************/
void UNI_SERVO::get_parameters(){
  Serial.println("舵机参数如下所示--------------------------------");
  Serial.print("当前定义的舵机数量为: ");
  Serial.print(_uni_servo_current_count);
  Serial.println(" 个舵机");
  Serial.print("当前定义的舵机引脚为: ");
  for(int i=0;i<_uni_servo_current_count;i++){
    Serial.print(_uni_servo_label[i]);Serial.print("  ");
  }Serial.println();
}
